﻿using System;
using System.Collections.Generic;
using System.Linq;
using UnityEngine;

namespace Yoozoo.Gameplay.Liberty
{
    
    public class VehicleSplineDebug : MonoBehaviour
    {
        public Rigidbody rigidbody;

        public List<Transform> targetPoints;

        public Transform frontPoint;

        public float endDistance = 2;

        public float startRotDistance = 3;
        
        public float force = 1;
        public float torque = 1;
        
        public float maxVelocity = 2;
        public float maxAngleVelocity = 10;

        private int _pointIndex = 0;
        
        private void FixedUpdate()
        {

            Transform target = targetPoints[_pointIndex];
            Vector3 frontPos = frontPoint.position;
            frontPos.y = 0;
            Vector3 tf = target.position - frontPos;

            
            
            if (tf.magnitude > endDistance)
            {
                AddForce(tf.normalized * force);
            }
            else
            {
                // 切换目标点
                if (_pointIndex + 1 < targetPoints.Count)
                {
                    _pointIndex++;
                    UnityEngine.Debug.LogError("切换目标点：" + _pointIndex);
                    
                    Transform target2 = targetPoints[_pointIndex];
                    Vector3 frontPos2 = frontPoint.position;
                    frontPos2.y = 0;
                    Vector3 tf2 = target2.position - frontPos2;
                    AddForce(tf2.normalized * force);
                }
                else
                {
                    //AddForce(Vector3.zero);
                }
               
            }

            if (Vector3.Distance(frontPoint.position, targetPoints[_pointIndex].position) <= startRotDistance)
            {
                Vector3 targetForward = GetPointDirection(_pointIndex);
                float angle = Mathf.Abs(Vector3.Angle(targetForward, transform.forward));

                if (angle > 2)
                {
                    float v = Vector3.Cross(transform.forward, targetForward).y;
                    if (v < 0)
                    {
                        // 左边
                        //AddTorque(-transform.up * torque * angle * 0.002f);
                        AddTorque(-transform.up * torque);
                    }
                    else
                    {
                        // 右边
                        //AddTorque(transform.up * torque * angle * 0.002f);
                        AddTorque(transform.up * torque);
                    }
                }
                else
                {
                    
                }
            }
        }


        public Vector3 GetPointDirection(int pointIndex)
        {
            if (pointIndex == 0)
            {
                return (targetPoints[pointIndex + 1].position - targetPoints[pointIndex].position).normalized;
            }
            else if (pointIndex == targetPoints.Count- 1)
            {
                return (targetPoints[pointIndex].position - targetPoints[pointIndex-1].position).normalized;
            }
            else
            {
                Vector3 v1 = (targetPoints[pointIndex + 1].position - targetPoints[pointIndex].position).normalized;
                Vector3 v2 = (targetPoints[pointIndex].position - targetPoints[pointIndex - 1].position).normalized;
                return ((v1 + v2) * 0.5f).normalized;
            }
        }

        public void AddForce(Vector3 velocity)
        {
            rigidbody.AddForce(velocity,ForceMode.VelocityChange);
        }


        public void AddTorque(Vector3 velocity)
        {
            rigidbody.AddTorque(velocity,ForceMode.VelocityChange);
        }


        private void OnDrawGizmos()
        {
            Gizmos.color = Color.yellow;
            Gizmos.DrawLine(transform.position, transform.position + transform.forward * 10);

            for (int i = 0; i < targetPoints.Count; i++)
            {
                if (i != targetPoints.Count - 1)
                {
                    Gizmos.DrawLine(targetPoints[i].position,targetPoints[i+1].position);
                }
            }
            
        }
    }
}